#ifndef __TRACKBALL_H__
#define __TRACKBALL_H__

Vector3 mapToTrackball(Vector3);
static bool mouseIsHeldDown=false;
static bool rightButtonDown=false;

static int moves=0;

Matrix4 trackball_rotation;

Vector3 mousePoints[2]={Vector3(0,0,0),Vector3(0,0,0)};

static int window_width;
static int window_height;

void funcyMouse(int button, int state, int, int)
{
        if (button == GLUT_RIGHT_BUTTON) {
                rightButtonDown=true;
        }

        if (button != GLUT_LEFT_BUTTON)
                return;

        if (state==GLUT_DOWN) {
                mouseIsHeldDown=true;
        }
        else {
                mouseIsHeldDown=false;
                moves=0;
        }

}

void funcyMotion(int px, int py) {
        if (!mouseIsHeldDown)
                return;

        moves++;
        mousePoints[0] = mousePoints[1];
        mousePoints[1] = mapToTrackball(Vector3(px,py,0));
        if (moves==1)
                return;

        if ((mousePoints[1]-mousePoints[0]).length() < 2) {
                mousePoints[1] = mousePoints[1] + mousePoints[0];
                return;
        }

        // Rotate around the cross of the two vectors                                                                                                                   
        Vector3 axis = Vector3::cross(mousePoints[0], mousePoints[1]);
        if (isnan(axis[0]) || isnan(axis[1]) || isnan(axis[2]))
                return;

        // Rotate with angle between the two vectors                                                                                                                    
        double dot = Vector3::dot(mousePoints[0], mousePoints[1]);
        double product = mousePoints[0].length()*mousePoints[1].length();
        double angle = acos(dot/product);
        axis.normalize();

        if (isnan(angle) || abs(angle)<0.0001)
                return;

        Matrix4 extraRot = Matrix4::createRotationMatrix(axis,angle);
        trackball_rotation = extraRot*trackball_rotation;
}

void setup_trackball(int w, int h) {
	window_width = w;
	window_height = h;
	trackball_rotation.identity();
	glutMouseFunc(funcyMouse);
	glutMotionFunc(funcyMotion);
}


Vector3 mapToTrackball(Vector3 point)
{
        Vector3 v;
        float d;
        // Scale                                                                                                                                                        
        v.v[0] = (2.0*point[0] - window_width) / window_width;
        v.v[1] = (window_height - 2.0*point[1]) / window_height;

        // Clamp to -1, 1                                                                                                                                               
        v.v[0] = max(-1.0,v.v[0]);
        v.v[0] = min(1.0,v.v[0]);

        v.v[1] = max(-1.0,v.v[1]);
        v.v[1] = min(1.0,v.v[1]);

        // Figure out what the z component should be                                                                                                                    
        // for the point (point.x point.y Z)                                                                                                                            
        // to be on the surface of the sphere                                                                                                                           
        v.v[2] = 0.0;
        d = v.length();
        d = (d<1.0) ? d : 1.0;
        v.v[2] = sqrt(1.001 - d*d);
        v.normalize();
        return v;
}


#endif
